/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Algorithm;

import java.awt.Point;
import java.awt.geom.Point2D;
import java.util.*;

/**
 *
 * @author ix
 */

public abstract class Pathfinding
{
    //point data structure, used to keep A* data for the algorithm
    private static class PData
    {
        public int x,y;     //co-ordinates
        public PData from;   //how we reached this point
        
        public double g,    //cost to reach the point
                      h,    //heuristic to the target
                      f;    //total cost through this point
        
        //1-line constructor
        public PData(int x, int y, PData from, double g, double h)
        { this.x = x; this.y = y; this.from = from; this.g = g; this.h = h; this.f = g + h; }
    }
    
    //Manhattan distance
    static public double GetDist(int ax, int ay, int bx, int by)
    {
        return Math.sqrt((ax-bx)*(ax-bx) + (ay-by)*(ay-by));
    }
    
    static public double GetDist(double ax, double ay, double bx, double by)
    {
        return Math.sqrt((ax-bx)*(ax-bx) + (ay-by)*(ay-by));
    }
    
    static public double GetDist2(Point2D a, Point2D b)
    {
        return (a.getX()-b.getX())*(a.getX()-b.getX()) + (a.getY()-b.getY())*(a.getY()-b.getY());
    }
    
    static public double GetDist(Point2D a, Point2D b)
    {
        return Math.sqrt((a.getX()-b.getX())*(a.getX()-b.getX()) + (a.getY()-b.getY())*(a.getY()-b.getY()));
    }
    static public double GetDist(Point a, Point b)
    {
        return Math.sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
    }
    
    //debug var; measures speed of running
    static long t_elapsed;
    
    //checks for LoS between 2 points
    static public boolean LoS(int ax, int ay, int bx, int by, boolean[][] field)
    {
        int dx = bx - ax,
            dy = by - ay,
            sx = 1,
            sy = 1;
        if(dx < 0)
        {
            dx = -dx;
            sx = -1;
        }
        if(dy < 0)
        {
            dy = -dy;
            sy = -1;
        }
        int ix = 1, iy = 1;
        int mx = 2 * dx,
            my = 2 * dy;
        while(ix < mx || iy < my)
        {
            if(iy > my || ix * my < iy * mx)
            {
                ax += sx;
                if(field[ax][ay]) 
                    return false;
                ix += 2;
            }
            else if(ix > mx || ix * my > iy * mx)
            {
                ay += sy;
                if(field[ax][ay]) 
                    return false;
                iy += 2;
            }
            else
            {
                if(field[ax][ay+sy] || field[ax+sx][ay] || field[ax+sx][ay + sy]) 
                    return false;
                ax += sx;
                ay += sy;
                ix += 2;
                iy += 2;
            }
        }
        return true;
    }
    
    static public double GetRouteLen(Point size, boolean[][] WalkableField, Point start, Point end)
    {
        List<Point> l = GetRoute(size, WalkableField, start, end);
        if(l.isEmpty())
            return 0;
        int d = 0;
        Iterator<Point> it = l.iterator();
        Point op = it.next(), p;
        while(it.hasNext())
        {
            p = it.next();
            d += GetDist(p, op);
            op = p;
        }
        return d;
    }
    static public List<Point> GetRoute(Point size, boolean[][] WalkableField, Point start, Point end)
    {
        if(start.x == end.x && start.y == end.y)
            return null;
        
        //heap-based priority queue
        PriorityQueue<PData> openSet = new PriorityQueue<PData>((int)GetDist(start.x, start.y, end.x, end.y), new Comparator<PData>() {
            //make it so it compares PDatas using their F value
            @Override
            public int compare(PData x, PData y)
            {
            if(x.f < y.f)
                return -1;
            if(x.f > y.f)
                return 1;
            return 0;
            }
        });
        
        PData[][] board = new PData[size.x][size.y];
        boolean[][] closed = new boolean[size.x][size.y];
        PData tp;
        
        PData p = new PData(start.x, start.y, null, 0, GetDist(start.x, start.y, end.x, end.y));
        p.from = p;
        board[start.x][start.y] = p;
        openSet.add(p);
        
        do  //while we have points in the open set
        {   
            p = openSet.poll(); //take the min-value
            closed[p.x][p.y] = true;    //add it to closed
            
            if(p.x == end.x && p.y == end.y)
                break;  //did we reach the destination? if so, break
            
            //for each point near the selected one
            for(int x=Math.max(p.x-1, 0); x < Math.min(p.x+2, size.x); x++)
                for(int y=Math.max(p.y-1, 0); y < Math.min(p.y+2, size.y); y++)
                {
                    if(!closed[x][y]
                       && LoS(x, y, p.x, p.y, WalkableField))
                    // if not closed and walkable
                    {
                        double cost = p.g       + GetDist(p.x, p.y, x, y);
                        double fcost = p.from.g + GetDist(p.from.x, p.from.y, x, y);
                        if(p.from != null && LoS(x, y, p.from.x, p.from.y, WalkableField) &&
                          fcost <= cost)
                            tp = new PData(x, y, p.from, fcost, GetDist(x, y, end.x, end.y));
                        else
                            tp = new PData(x, y, p, cost, GetDist(x, y, end.x, end.y));
                        
                        //create PData for this point
                        //tp = new PData(x, y, p, cost, GetH(x, y, end.x, end.y, d));
                        
                        //if the point isn't checked or current value < previous value
                        if(board[x][y] == null || tp.f < board[x][y].f)
                        {
                            //remove previous one, add new one
                            openSet.remove(board[x][y]);
                            openSet.add(tp);
                            board[x][y] = tp;
                        }
                    }
                }
            
        }
        while(!openSet.isEmpty());
        
        //if we haven't reached the end, terminate (no path found)
        if(p.x != end.x || p.y != end.y)
        {
            return null;
        }
        
        //otherwise go back through the path, recording it on our way
        LinkedList<Point> path = new LinkedList<Point>();
        path.addFirst(new Point(p.x, p.y)); //add last point
        p = p.from;  //add next points
        while(p.x != start.x || p.y != start.y)
        {
            path.addFirst(new Point(p.x, p.y));
            p = p.from;  //add next points
        }   //till we go back to the start
        
        return (List<Point>)path;
    }
}
